#!/usr/bin/env python 
# -*- coding: utf-8 -*-
 
import rospy
import actionlib
import os
from actionlib_msgs.msg import *
from geometry_msgs.msg import Pose, Point, Quaternion, Twist
from move_base_msgs.msg import MoveBaseAction, MoveBaseGoal
from tf.transformations import quaternion_from_euler
from math import pi
from std_msgs.msg import String
from object_msgs.msg import ObjectsInBoxes
from sensor_msgs.msg import RegionOfInterest as ROI
from ros_arduino_msgs.srv import DigitalWrite
from ros_arduino_msgs.srv import DigitalSetDirection
findcat=0
roi_data=ROI()
stop_nav=1
object_count=0


#初始化节点
rospy.init_node('challenge',anonymous=False)
# rospy.on_shutdown(self.shutdown)
pub = rospy.Publisher('roi', ROI, queue_size=10)
twist_pub=rospy.Publisher('cmd_vel',Twist,queue_size=10)
r = rospy.Rate(3) 
#设置参数 
square_size = rospy.get_param('~square_size',1.0)
#添加坐标点,输入x（前）坐标，y（左）坐标，th（平面朝向0～360度）
#action服务器连接
cmd_vel_pub = rospy.Publisher('cmd_vel', Twist, queue_size=5)
move_base = actionlib.SimpleActionClient("move_base", MoveBaseAction)
rospy.loginfo('等待move_base action服务器连接...')
move_base.wait_for_server(rospy.Duration(30))
rospy.loginfo('已连接导航服务')

def nav_to(x,y,d):
    goal = MoveBaseGoal()
    goal.target_pose.header.frame_id='map'
    goal.target_pose.header.stamp = rospy.Time.now()
    goal.target_pose.pose = pose_e(x,y,d)
    move(goal)

#写一个函数 用于任务完成提示。
def move(goal):
    move_base.send_goal(goal)
    finished_within_time = findcat or move_base.wait_for_result(rospy.Duration(60))
    if  finished_within_time:
        move_base.cancel_goal()
        rospy.loginfo('时间超时，或已发现目标，导航任务取消。')
	return 
    state = move_base.get_state()
    if state == GoalStatus.SUCCEEDED:
	rospy.loginfo('导航成功！')
    
def shutdown(self):
    rospy.loginfo('机器人任务停止')
    move_base.cancel_goal()
    rospy.sleep(2)
    cmd_vel_pub.publish(Twist)
    rospy.sleep(1)
    
def pose_e(x,y,th):#输入x（前）坐标，y（左）坐标，th（平面朝向0～360度）
    new_pose=Pose()
    new_pose.position.x=float(x)
    new_pose.position.y=float(y)
    #机器朝向，平面朝向弧度转化成四元数空间位姿
    q=quaternion_from_euler(0.0,0.0,float(th)/180.0*pi)
    new_pose.orientation.x=q[0]
    new_pose.orientation.y=q[1]
    new_pose.orientation.z=q[2]
    new_pose.orientation.w=q[3]
    return  new_pose


def callback(data):
    global findcat
    global roi_data
    global stop_nav
    global object_count
    data = data.objects_vector
    if data:        
        for i in data:
            p = i.object.probability
            object_name = i.object.object_name
            if p >= 0.90:
                if object_name == 'cat':
		    object_count+=1
                    roi_data=i.roi
		    pub.publish(roi_data)
		    if stop_nav==1 and object_count>10:
			findcat=1
			move_base.cancel_goal()
			cmd_vel_pub.publish(Twist())
			rospy.loginfo('发现目标，停止导航')
			stop_nav=0
			
def get_data():
    rospy.Subscriber("/movidius_ncs_nodelet/detected_objects_multiple", ObjectsInBoxes, callback, queue_size=1)
if __name__ == '__main__':
    get_data()
    while findcat==0:
        nav_to(1.5,0.5,-90)
        nav_to(1.5,-0.5,90)
        nav_to(0,0,0)
    #move_base.cancel_goal()
    os.system("play ~/Desktop/demo_code/mp3/findcat.mp3 ")
    
    os.system("python object_tracker.py")  
    time_last=rospy.get_time()
    while rospy.get_time()-time_last<2:
        vel_data=Twist()
        vel_data.linear.x=0.2
        cmd_vel_pub.publish(vel_data)
    cmd_vel_pub.publish(Twist())
    print("接近目标")  
    rospy.wait_for_service('/mx_chassis/digital_write')
    rospy.wait_for_service('/mx_chassis/digital_set_direction')
    dr=rospy.ServiceProxy('mx_chassis/digital_set_direction',DigitalSetDirection)
    dw=rospy.ServiceProxy('/mx_chassis/digital_write',DigitalWrite)
    dr(9,1)
    dw(9,1) 
    print("9 pin is high")  
    rospy.spin()
    
       

        







